﻿#region Copyright
// Copyright (c) 2011-2012 RaisingForce Team (Alikin Sergey)
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following
// conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
// OTHER DEALINGS IN THE SOFTWARE.
#endregion

using System;
using System.Diagnostics.Contracts;
using System.Threading;
using NKH.MindSqualls;
using NKH.MindSqualls.MotorControl;
using RaisingForce.Raise.Hardware.LegoTribot.Properties;

namespace RaisingForce.Raise.Hardware.LegoTribot
{
    [ContractClass(typeof(LegoTribotHardwareBaseContract))]
    public abstract class LegoTribotHardwareBase :
        RobotHardware
    {
        #region Contructors

        protected LegoTribotHardwareBase()
            : base()
        {
            // does nothing
        }

        #endregion

        #region Private Declarations

        private McNxtBrick brick;

        /// <summary>
        /// Synchronization object for physical access to robot.
        /// </summary>
        private readonly object robotSyncObject = new object();

        #endregion

        #region Protected Abstract Declarations

        protected abstract McNxtBrick CreateBrick(
            IRobotHardwareOptions options);

        #endregion

        #region Base Overrides

        sealed protected override void InitializeHardware(
            IRobotHardwareOptions options,
            object hardwareSyncObject,
            out IRobotEngine engine,
            out IRobotSensor sensor)
        {   // exceptions are handled in base class
            McNxtMotor leftMotor = new McNxtMotor();
            McNxtMotor rightMotor = new McNxtMotor();
            NxtUltrasonicSensor ultrasonicSensor =
                new NxtUltrasonicSensor();
            McNxtBrick brick = this.CreateBrick(options);
            brick.MotorB = leftMotor;
            brick.MotorC = rightMotor;
            brick.Sensor4 = ultrasonicSensor;
            IRobotEngineOptions engineOptions =
                new ReadOnlyRobotEngineOptions(options);
            engine = new LegoTribotEngine(
                hardwareSyncObject,
                this.robotSyncObject,
                leftMotor,
                rightMotor,
                engineOptions);
            sensor = new LegoTribotSensor(
                hardwareSyncObject,
                this.robotSyncObject,
                ultrasonicSensor,
                options.ObstacleDistance);
            this.brick = brick;
        }

        sealed protected override void ConnectHardware()
        {
            // delay time, if MotorControl program is not started on robot hardware:
            const int motorControlDelayInMs = 500;
            if (this.brick != null)
            {
                // connecting to NXT brick
                try
                {
                    if (!this.brick.IsConnected)
                    {
                        this.brick.Connect();
                    }
                }
                catch (Exception ex)
                {
                    throw new RobotHardwareException(
                        RobotHardwareFailureReason.Hardware,
                        Resources.Exception_BrickConnectionFailure,
                        innerException: ex);
                }
                // starting MotorControl program
                if (!this.brick.IsMotorControlOnNxt())
                {
                    throw new RobotHardwareException(
                        RobotHardwareFailureReason.Hardware,
                        Resources.Exception_MotorControlNotFound);
                }
                if (!this.brick.IsMotorControlRunning())
                {
                    try
                    {
                        this.brick.StartMotorControl();
                        // wait until MotorControl starts
                        Thread.CurrentThread.Join(
                            TimeSpan.FromMilliseconds(motorControlDelayInMs));
                    }
                    catch (Exception ex)
                    {
                        throw new RobotHardwareException(
                            RobotHardwareFailureReason.Hardware,
                            Resources.Exception_MotorControlError,
                            innerException: ex);
                    }
                }
            }
        }

        sealed protected override void DisconnectHardware()
        {
            if (this.brick != null && this.brick.IsConnected)
            {
                try
                {
                    this.brick.Disconnect();
                }
                catch (Exception ex)
                {
                    throw new RobotHardwareException(
                        RobotHardwareFailureReason.Hardware,
                        Resources.Exception_BrickDisconnectionFailure,
                        innerException: ex);
                }
            }
        }

        #endregion
    }
}
